//
// Created by fanghr on 18-3-30.
//
#include "../include/thread_disp.h"

static void *cacl_disp(void *obj);

thread_disp::thread_disp(thread_mat &l, thread_mat &r) : mat_l(l), mat_r(r) {
    pthread_rwlock_init(&rwlock_disp, nullptr);
    id_disp_ready = is_run = false;
}

thread_disp::~thread_disp() {
    stop();
    pthread_rwlock_destroy(&rwlock_disp);
}

int thread_disp::run() {
    if (is_run)
        return 0;
    if (pthread_create(&thread, nullptr, cacl_disp, this))
        return -1;
    is_run = 1;
    return 0;
}

int thread_disp::stop() {
    if (!is_run)
        return 0;
    if (pthread_cancel(thread))
        return -1;
    is_run = false;
    return 0;
}

static void *cacl_disp(void *obj) {
    auto ptr = (thread_disp *) obj;
    do {
        pthread_rwlock_rdlock(&ptr->mat_r.to_gray.rwlock_to_gray);
        if (ptr->mat_r.to_gray.is_gray_mat_ready) {
            pthread_rwlock_unlock(&ptr->mat_r.to_gray.rwlock_to_gray);
            break;
        }
        pthread_rwlock_unlock(&ptr->mat_r.to_gray.rwlock_to_gray);
    } while (true);
    do {
        pthread_rwlock_rdlock(&ptr->mat_l.to_gray.rwlock_to_gray);
        if (ptr->mat_l.to_gray.is_gray_mat_ready) {
            pthread_rwlock_unlock(&ptr->mat_l.to_gray.rwlock_to_gray);
            break;
        }
        pthread_rwlock_unlock(&ptr->mat_l.to_gray.rwlock_to_gray);
    } while (true);
    pthread_rwlock_rdlock(&ptr->mat_r.to_gray.rwlock_to_gray);
    pthread_rwlock_rdlock(&ptr->mat_l.to_gray.rwlock_to_gray);
    if (ptr->mat_l.to_gray.mat_gray.empty() || ptr->mat_r.to_gray.mat_gray.empty()) {
        pthread_rwlock_unlock(&ptr->mat_r.to_gray.rwlock_to_gray);
        pthread_rwlock_unlock(&ptr->mat_l.to_gray.rwlock_to_gray);
        ptr->stop();
        return nullptr;
    }
    if (ptr->mat_l.to_gray.mat_gray.size != ptr->mat_r.to_gray.mat_gray.size) {
        pthread_rwlock_unlock(&ptr->mat_r.to_gray.rwlock_to_gray);
        pthread_rwlock_unlock(&ptr->mat_l.to_gray.rwlock_to_gray);
        ptr->stop();
        return nullptr;
    }
    pthread_rwlock_unlock(&ptr->mat_r.to_gray.rwlock_to_gray);
    pthread_rwlock_unlock(&ptr->mat_l.to_gray.rwlock_to_gray);

    cv::StereoSGBM sgbm(-64, 128, 41);
    cv::Mat tmp_l, tmp_r, disp;

    while (true) {
        pthread_rwlock_rdlock(&ptr->mat_r.to_gray.rwlock_to_gray);
        pthread_rwlock_rdlock(&ptr->mat_l.to_gray.rwlock_to_gray);
        ptr->mat_l.to_gray.mat_gray.copyTo(tmp_l);
        ptr->mat_r.to_gray.mat_gray.copyTo(tmp_r);
        pthread_rwlock_unlock(&ptr->mat_r.to_gray.rwlock_to_gray);
        pthread_rwlock_unlock(&ptr->mat_l.to_gray.rwlock_to_gray);
        pthread_rwlock_wrlock(&ptr->rwlock_disp);
        sgbm(tmp_l, tmp_r, disp);
        ptr->id_disp_ready = true;
        disp.convertTo(ptr->disp8, CV_8U, 255 / (128 * 16.));
        pthread_rwlock_unlock(&ptr->rwlock_disp);
    }

}